#include <iostream>
#include <fstream>
#include <cstdlib>
#include <cstring>
#include <string>
#include <sstream>

#include "../CameraInterface.h"
#include "../LFOSCommon.h"

using namespace std;

//typedef struct
//{
//	unsigned char R;
//	unsigned char G;
//	unsigned char B;
//} Color;

int main4(int argc, char* argv[])
{
	CameraInterface ci;
	unsigned char* image;
	unsigned char outHeader[1078];
	unsigned char* outBuffer;
	ofstream outFile;
	int size;

	if(argc != 2)
	{
		cout << "Usage: CameraInterfaceTest <output base>\n";
		exit(1);
	}

//	outFile.open(argv[1], ios::out | ios::binary);
//	if(outFile.fail())
//	{
//		cout << "Error opening " << argv[1] << endl;
//		exit(1);
//	}

	if(!ci.Initialize(5000))
	{
		cout << "Error initializing camera driver.\n";
		exit(1);
	}

    //create output bitmap header
    size = 1078 + ROWS*(COLS + ((COLS) % 4));
    //magic number
    outHeader[0] = 66;
    outHeader[1] = 77;
    //size in little endian
    outHeader[2] = size & 0xFF;
    outHeader[3] = (size >> 8) & 0xFF;
    outHeader[4] = (size >> 16) & 0xFF;
    outHeader[5] = (size >> 24) & 0xFF;
    //next four bytes unused
    outHeader[6] = 0;
    outHeader[7] = 0;
    outHeader[8] = 0;
    outHeader[9] = 0;
    //pixel array offset - 54 in little endian
    outHeader[10] = 54;
    outHeader[11] = 4;
    outHeader[12] = 0;
    outHeader[13] = 0;
    //number of bytes in DIB Header - 40 in little endian
    outHeader[14] = 40;
    outHeader[15] = 0;
    outHeader[16] = 0;
    outHeader[17] = 0;
    //width in little endian
    outHeader[18] = COLS & 0xFF;
    outHeader[19] = (COLS >> 8) & 0xFF;
    outHeader[20] = (COLS >> 16) & 0xFF;
    outHeader[21] = (COLS >> 24) & 0xFF;
    //Height in little endian
    outHeader[22] = ROWS & 0xFF;
    outHeader[23] = (ROWS >> 8) & 0xFF;
    outHeader[24] = (ROWS >> 16) & 0xFF;
    outHeader[25] = (ROWS >> 24) & 0xFF;
    //number of color planes - 1 in little endian
    outHeader[26] = 1;
    outHeader[27] = 0;
    //Number of bits per pixel - 8 in little endian
    outHeader[28] = 8;
    outHeader[29] = 0;
    //no Compression
    outHeader[30] = 0;
    outHeader[31] = 0;
    outHeader[32] = 0;
    outHeader[33] = 0;
    //size of image in little endian
    outHeader[34] = (size - 1078) & 0xFF;
    outHeader[35] = ((size - 1078) >> 8) & 0xFF;
    outHeader[36] = ((size - 1078) >> 16) & 0xFF;
    outHeader[37] = ((size - 1078) >> 24) & 0xFF;
    //horizontal resolution - 2835 pixels/meter
    //outHeader[38] = 19;
    //outHeader[39] = 11;
    outHeader[38] = 0;
    outHeader[39] = 0;
    outHeader[40] = 0;
    outHeader[41] = 0;
    //vertical resolution - 2835 pixels/meter
    //outHeader[42] = 19;
    //outHeader[43] = 11;
    outHeader[42] = 0;
    outHeader[43] = 0;
    outHeader[44] = 0;
    outHeader[45] = 0;
    //0 colors in palette
    outHeader[46] = 0;
    outHeader[47] = 1;
    outHeader[48] = 0;
    outHeader[49] = 0;
    //0 important colors
    outHeader[50] = 0;
    outHeader[51] = 0;
    outHeader[52] = 0;
    outHeader[53] = 0;

    //create color table
    for(int i=0; i<256; i++)
    {
    	outHeader[54+4*i] = i;
    	outHeader[54+4*i+1] = i;
    	outHeader[54+4*i+2] = i;
    	outHeader[54+4*i+3] = 0;
    }

    outBuffer = new unsigned char[size-1078];

//    int buffer_size = fmt.fmt.pix.sizeimage;
//    cout << buffer_size << endl;

    int count = 0;
    stringstream ss;
    string fileName;
    time_t rawTime;
	struct tm* currentTimeBD;
    while(count < 5)
    {
    	image = ci.CaptureFrame();
    	if(image != 0)
    	{
    		time(&rawTime);
            currentTimeBD = localtime(&rawTime);

			for(int y=0; y < ROWS; y++)
			{
				for(int x=0; x < COLS; x++)
				{
					int inIndex = ROW_COL_TO_INDEX(x,y);
					int outIndex = ROW_COL_TO_INDEX(x, ROWS-y-1);
					outBuffer[outIndex] = image[inIndex];
				}
				//if row doesn't fall on 4 byte boundary - pad with zeros
				if(((COLS*3) % 4) != 0)
				{
					int index = ROW_COL_TO_INDEX(COLS, ROWS-y-1);
					for(int i = 0; i < (COLS)%4; i++)
						outBuffer[index+i] = 0;
				}
			}

			//fileName = <output base>_<count>_<hour>_<min>_<sec>.bmp
            ss.str("");
			ss << argv[1] << "_" << count << "_" << currentTimeBD->tm_hour << "_" << currentTimeBD->tm_min <<
					"_" << currentTimeBD->tm_sec << ".bmp";
			fileName = ss.str();
            cout << fileName << endl;;
			outFile.open(fileName.c_str(), ios::out | ios::binary);

			//write data to file
			outFile.write((char*)outHeader, 1078);
			outFile.write((char*)outBuffer, size-1078);
    
            cout << "Image " << count + 1 << " captured\n";
            count++;
            outFile.close();
    	}
    }

	cout << "Camera Interface Test Complete" << endl;

	return 0;
}

